/*
Copyright 2007 Luke Benstead

This file is part of slMath.

slMath is free software; you can redistribute it and/or modify
it under the terms of the GNU Lesser Public License as published by
the Free Software Foundation; either version 3 of the License, or
(at your option) any later version.

slMath is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
GNU Lesser Public License for more details.

You should have received a copy of the GNU Lesser Public License
along with this program.  If not, see <http://www.gnu.org/licenses/>.

*/
#include "math/utility.h"
#include "math/mat4.h"
#include "math/vec3.h"
#include "math/quaternion.h"

#ifdef __CPLUSPLUS
	#include <memory>
	#include <cassert>
#else
	#include <memory.h>
	#include <assert.h>
#endif

/** Sets pOut to an identity matrix returns pOut*/
slMat4* slMat4Identity(slMat4* pOut)
{
	memset(pOut->m_Mat, 0, sizeof(float) * 16);
	pOut->m_Mat[0] = pOut->m_Mat[5] = pOut->m_Mat[10] = pOut->m_Mat[15] = 1.0f;
	return pOut;
}

slMat4* slMat4Inverse(slMat4* pOut, slScalar* pDeterminate, const slMat4* pM)
{
	assert(0);

	return pOut;
}

/** Returns true if pIn is an identity matrix */
bool  slMat4IsIdentity(const slMat4* pIn)
{
	static const float identity [] = { 	1.0f, 0.0f, 0.0f, 0.0f,
										0.0f, 1.0f, 0.0f, 0.0f,
										0.0f, 0.0f, 1.0f, 0.0f,
										0.0f, 0.0f, 0.0f, 1.0f };

	return (memcmp(identity, pIn->m_Mat, sizeof(float) * 16) == 0);
}

/** Sets pOut to the transpose of pIn, returns pOut */
slMat4* slMat4Transpose(slMat4* pOut, const slMat4* pIn)
{
	for (int z = 0; z < 4; ++z)
		for (int x = 0; x < 4; ++x)
			pOut->m_Mat[(z * 4) + x] = pIn->m_Mat[(x * 4) + z];

	return pOut;
}

/* Multiplies pM1 with pM2, stores the result in pOut, returns pOut */
slMat4* slMat4Multiply(slMat4* pOut, const slMat4* pM1, const slMat4* pM2)
{
	float mat[16];

	const float *m1 = pM1->m_Mat, *m2 = pM2->m_Mat;

	mat[0] = m1[0] * m2[0] + m1[4] * m2[1] + m1[8] * m2[2] + m1[12] * m2[3];
	mat[1] = m1[1] * m2[0] + m1[5] * m2[1] + m1[9] * m2[2] + m1[13] * m2[3];
	mat[2] = m1[2] * m2[0] + m1[6] * m2[1] + m1[10] * m2[2] + m1[14] * m2[3];
	mat[3] = m1[3] * m2[0] + m1[7] * m2[1] + m1[11] * m2[2] + m1[15] * m2[3];

	mat[4] = m1[0] * m2[4] + m1[4] * m2[5] + m1[8] * m2[6] + m1[12] * m2[7];
	mat[5] = m1[1] * m2[4] + m1[5] * m2[5] + m1[9] * m2[6] + m1[13] * m2[7];
	mat[6] = m1[2] * m2[4] + m1[6] * m2[5] + m1[10] * m2[6] + m1[14] * m2[7];
	mat[7] = m1[3] * m2[4] + m1[7] * m2[5] + m1[11] * m2[6] + m1[15] * m2[7];

	mat[8] = m1[0] * m2[8] + m1[4] * m2[9] + m1[8] * m2[10] + m1[12] * m2[11];
	mat[9] = m1[1] * m2[8] + m1[5] * m2[9] + m1[9] * m2[10] + m1[13] * m2[11];
	mat[10] = m1[2] * m2[8] + m1[6] * m2[9] + m1[10] * m2[10] + m1[14] * m2[11];
	mat[11] = m1[3] * m2[8] + m1[7] * m2[9] + m1[11] * m2[10] + m1[15] * m2[11];

	mat[12] = m1[0] * m2[12] + m1[4] * m2[13] + m1[8] * m2[14] + m1[12] * m2[15];
	mat[13] = m1[1] * m2[12] + m1[5] * m2[13] + m1[9] * m2[14] + m1[13] * m2[15];
	mat[14] = m1[2] * m2[12] + m1[6] * m2[13] + m1[10] * m2[14] + m1[14] * m2[15];
	mat[15] = m1[3] * m2[12] + m1[7] * m2[13] + m1[11] * m2[14] + m1[15] * m2[15];


	memcpy(pOut->m_Mat, mat, sizeof(float)*16);

	return pOut;
}

slMat4* slMat4MultiplyTranspose(slMat4* pOut, const slMat4* pM1, const slMat4* pM2)
{
	assert(0);
	return pOut;
}

/** Assigns the value of pIn to pOut */
slMat4* slMat4Assign(slMat4* pOut, const slMat4* pIn)
{
	assert(pOut != pIn); //You have tried to self-assign!!

	memcpy(pOut->m_Mat, pIn->m_Mat, sizeof(float)*16);

	return pOut;
}

/** Returns true if the 2 matrices are equal (approximately) */
bool slMat4AreEqual(const slMat4* pMat1, const slMat4* pMat2)
{
	assert(pMat1 != pMat2); //You are comparing the same thing!

	for (int i = 0; i < 16; ++i)
	{
		if (!(pMat1->m_Mat[i] + slEpsilon > pMat2->m_Mat[i] &&
			  pMat1->m_Mat[i] - slEpsilon < pMat2->m_Mat[i]))
			return false;
	}

	return true;
}

/// Build a matrix from an axis and an angle. Result is stored in pOut. pOut is returned.
slMat4* slMat4RotationAxis(slMat4* pOut, const slVec3* axis, slScalar radians)
{
	float rcos = slCos(radians);
	float rsin = slSin(radians);

	pOut->m_Mat[0] = rcos + axis->m_X * axis->m_X * (1 - rcos);
	pOut->m_Mat[1] = axis->m_Z * rsin + axis->m_Y * axis->m_X * (1 - rcos);
	pOut->m_Mat[2] = -axis->m_Y * rsin + axis->m_Z * axis->m_X * (1 - rcos);
	pOut->m_Mat[3] = 0.0f;

	pOut->m_Mat[4] = -axis->m_Z * rsin + axis->m_X * axis->m_Y *(1-rcos);
	pOut->m_Mat[5] = rcos + axis->m_Y * axis->m_Y * (1 - rcos);
	pOut->m_Mat[6] = axis->m_X * rsin + axis->m_Z * axis->m_Y * (1 - rcos);
	pOut->m_Mat[7] = 0.0f;

	pOut->m_Mat[8] = axis->m_Y * rsin + axis->m_X * axis->m_Z *(1-rcos);
	pOut->m_Mat[9] = -axis->m_X * rsin + axis->m_Y * axis->m_Z *(1-rcos);
	pOut->m_Mat[10] = rcos + axis->m_Z * axis->m_Z * (1 - rcos);
	pOut->m_Mat[11] = 0.0f;

	pOut->m_Mat[12] = 0.0f;
	pOut->m_Mat[13] = 0.0f;
	pOut->m_Mat[14] = 0.0f;
	pOut->m_Mat[15] = 1.0f;

	return pOut;
}

/// Builds an X-axis rotation matrix and stores it in pOut, returns pOut
slMat4* slMat4RotationX(slMat4* pOut, const float radians)
{
	/*
		 |  1  0       0       0 |
     M = |  0  cos(A) -sin(A)  0 |
         |  0  sin(A)  cos(A)  0 |
         |  0  0       0       1 |

	*/

	pOut->m_Mat[0] = 1.0f;
	pOut->m_Mat[1] = 0.0f;
	pOut->m_Mat[2] = 0.0f;
	pOut->m_Mat[3] = 0.0f;

	pOut->m_Mat[4] = 0.0f;
	pOut->m_Mat[5] = slCos(radians);
	pOut->m_Mat[6] = slSin(radians);
	pOut->m_Mat[7] = 0.0f;

	pOut->m_Mat[8] = 0.0f;
	pOut->m_Mat[9] = -slSin(radians);
	pOut->m_Mat[10] = slCos(radians);
	pOut->m_Mat[11] = 0.0f;

	pOut->m_Mat[12] = 0.0f;
	pOut->m_Mat[13] = 0.0f;
	pOut->m_Mat[14] = 0.0f;
	pOut->m_Mat[15] = 1.0f;

	return pOut;
}

slMat4* slMat4RotationY(slMat4* pOut, const float radians)
{
	/*
         |  cos(A)  0   sin(A)  0 |
     M = |  0       1   0       0 |
         | -sin(A)  0   cos(A)  0 |
         |  0       0   0       1 |
	*/

	pOut->m_Mat[0] = slCos(radians);
	pOut->m_Mat[1] = 0.0f;
	pOut->m_Mat[2] = -slSin(radians);
	pOut->m_Mat[3] = 0.0f;

	pOut->m_Mat[4] = 0.0f;
	pOut->m_Mat[5] = 1.0f;
	pOut->m_Mat[6] = 0.0f;
	pOut->m_Mat[7] = 0.0f;

	pOut->m_Mat[8] = slSin(radians);
	pOut->m_Mat[9] = 0.0f;
	pOut->m_Mat[10] = slCos(radians);
	pOut->m_Mat[11] = 0.0f;

	pOut->m_Mat[12] = 0.0f;
	pOut->m_Mat[13] = 0.0f;
	pOut->m_Mat[14] = 0.0f;
	pOut->m_Mat[15] = 1.0f;

	return pOut;
}

slMat4* slMat4RotationZ(slMat4* pOut, const float radians)
{
	/*
         |  cos(A)  -sin(A)   0   0 |
     M = |  sin(A)   cos(A)   0   0 |
         |  0        0        1   0 |
         |  0        0        0   1 |
	*/

	pOut->m_Mat[0] = slCos(radians);
	pOut->m_Mat[1] = slSin(radians);
	pOut->m_Mat[2] = 0.0f;
	pOut->m_Mat[3] = 0.0f;

	pOut->m_Mat[4] = -slSin(radians);;
	pOut->m_Mat[5] = slCos(radians);
	pOut->m_Mat[6] = 0.0f;
	pOut->m_Mat[7] = 0.0f;

	pOut->m_Mat[8] = 0.0f;
	pOut->m_Mat[9] = 0.0f;
	pOut->m_Mat[10] = 1.0f;
	pOut->m_Mat[11] = 0.0f;

	pOut->m_Mat[12] = 0.0f;
	pOut->m_Mat[13] = 0.0f;
	pOut->m_Mat[14] = 0.0f;
	pOut->m_Mat[15] = 1.0f;

	return pOut;
}

slMat4* slMat4RotationPitchYawRoll(slMat4* pOut, const slScalar pitch, const slScalar yaw, const slScalar roll)
{
	double cr = cos(pitch);
	double sr = sin(pitch);
	double cp = cos(yaw);
	double sp = sin(yaw);
	double cy = cos(roll);
	double sy = sin(roll);

	pOut->m_Mat[0] = (slScalar) cp * cy;
	pOut->m_Mat[4] = (slScalar) cp * sy;
	pOut->m_Mat[8] = (slScalar) -sp;

	double srsp = sr * sp;
	double crsp = cr * sp;

	pOut->m_Mat[1] = (slScalar) srsp * cy - cr * sy;
	pOut->m_Mat[5] = (slScalar) srsp * sy + cr * cy;
	pOut->m_Mat[9] = (slScalar) sr * cp;

	pOut->m_Mat[2] = (slScalar) crsp * cy + sr * sy;
	pOut->m_Mat[6] = (slScalar) crsp * sy - sr * cy;
	pOut->m_Mat[10] = (slScalar) cr * cp;

	pOut->m_Mat[3] = pOut->m_Mat[7] = pOut->m_Mat[11] = 0.0;
	pOut->m_Mat[15] = 1.0;

	return pOut;
}

/** Converts a quaternion to a rotation matrix, stored in pOut, returns pOut */
slMat4* slMat4RotationQuaternion(slMat4* pOut, const slQuaternion* pQ)
{
	pOut->m_Mat[ 0] = 1.0f - 2.0f * (pQ->m_Y * pQ->m_Y + pQ->m_Z * pQ->m_Z );
	pOut->m_Mat[ 4] = 2.0f * (pQ->m_X * pQ->m_Y + pQ->m_Z * pQ->m_W);
	pOut->m_Mat[ 8] = 2.0f * (pQ->m_X * pQ->m_Z - pQ->m_Y * pQ->m_W);
	pOut->m_Mat[12] = 0.0f;

	// Second row
	pOut->m_Mat[ 1] = 2.0f * ( pQ->m_X * pQ->m_Y - pQ->m_Z * pQ->m_W );
	pOut->m_Mat[ 5] = 1.0f - 2.0f * ( pQ->m_X * pQ->m_X + pQ->m_Z * pQ->m_Z );
	pOut->m_Mat[ 9] = 2.0f * (pQ->m_Z * pQ->m_Y + pQ->m_X * pQ->m_W );
	pOut->m_Mat[13] = 0.0f;

	// Third row
	pOut->m_Mat[ 2] = 2.0f * ( pQ->m_X * pQ->m_Z + pQ->m_Y * pQ->m_W );
	pOut->m_Mat[ 6] = 2.0f * ( pQ->m_Y * pQ->m_Z - pQ->m_X * pQ->m_W );
	pOut->m_Mat[10] = 1.0f - 2.0f * ( pQ->m_X * pQ->m_X + pQ->m_Y * pQ->m_Y );
	pOut->m_Mat[14] = 0.0f;

	// Fourth row
	pOut->m_Mat[ 3] = 0;
	pOut->m_Mat[ 7] = 0;
	pOut->m_Mat[11] = 0;
	pOut->m_Mat[15] = 1.0f;

	return pOut;
}

/** Builds a scaling matrix */
slMat4* slMat4Scaling(slMat4* pOut, const slScalar x, const slScalar y, const slScalar z)
{
	memset(pOut->m_Mat, 0, sizeof(float) * 16);
	pOut->m_Mat[0] = x;
	pOut->m_Mat[5] = y;
	pOut->m_Mat[10] = z;
	pOut->m_Mat[15] = 1.0f;

	return pOut;
}

slMat4* slMat4Translation(slMat4* pOut, const slScalar x, const slScalar y, const slScalar z)
{
	//FIXME: Write a test for this
	//assert(0);
	memset(pOut->m_Mat, 0, sizeof(float) * 16);

	pOut->m_Mat[12] = x;
	pOut->m_Mat[13] = y;
	pOut->m_Mat[14] = z;

	return pOut;
}

slVec3* slMat4GetUpVec3(slVec3* pOut, const slMat4* pIn)
{
	pOut->m_X = pIn->m_Mat[4];
	pOut->m_Y = pIn->m_Mat[5];
	pOut->m_Z = pIn->m_Mat[6];

	slVec3Normalize(pOut, pOut);

	return pOut;
}

slVec3* slMat4GetRightVec3(slVec3* pOut, const slMat4* pIn)
{
	pOut->m_X = pIn->m_Mat[0];
	pOut->m_Y = pIn->m_Mat[1];
	pOut->m_Z = pIn->m_Mat[2];

	slVec3Normalize(pOut, pOut);

	return pOut;
}

slVec3* slMat4GetForwardVec3(slVec3* pOut, const slMat4* pIn)
{
	pOut->m_X = pIn->m_Mat[8];
	pOut->m_Y = pIn->m_Mat[9];
	pOut->m_Z = pIn->m_Mat[10];

	slVec3Normalize(pOut, pOut);

	return pOut;
}

